package TrainModel;

import Utilities.logFileIO;
import Utilities.TrainConstants;

public class PhysicsEngine
{
	private final int tickDivideBy = 10;
	
	private boolean bEngineFail;
	private boolean bBrakeFail;
	
	private double currVelocity; // in m/s
	//private double currWeight;
	//private double currPosition;
	
	private boolean bBraking;
	public boolean bEBraking;
	
	public double powerInput;
	public double brakeInput;
	public double dCurrentAccel;
	
	public boolean bWheelSlip;
	
	private double dFrictionCoeffStatic;
	private double dFrictionCoeffKinetic;
	
	private logFileIO logFile;
	private int iTrainID;
	
	public PhysicsEngine( logFileIO lf, int tID )
	{
		logFile = lf;
		logFile.write("Train " + iTrainID + ": PhysEng: I am a Physics Engine!");
		currVelocity = 0;
		//currPosition = 0;
		//currWeight = 0;
		
		bBraking = false;
		powerInput = 0;
		brakeInput = 0;
		
		bEngineFail = false;
		bBrakeFail = false;
		bWheelSlip = false;
		dFrictionCoeffKinetic = 1;
		dFrictionCoeffStatic = 1;
		iTrainID = tID;
	}
	
	public void setFrictionCoeffStatic( double c )
	{
		dFrictionCoeffStatic = c;
	}
	
	public void setFrictionCoeffKinetic( double c )
	{
		dFrictionCoeffKinetic = c;
	}
	
	public void failEngine( boolean f )
	{
		bEngineFail = f;
	}
	
	public void failBrakes( boolean f )
	{
		bBrakeFail = f;
	}
	
	public boolean getEngineFail()
	{
		return bEngineFail;
	}
	
	public boolean getBrakeFail()
	{
		return bBrakeFail;
	}
	
	public void setPower( double p )
	{
		bBraking = false;
		bEBraking = false;
		brakeInput = 0;
		
		if( bEngineFail )
		{
			powerInput = 0;
		}
		else if( p >= 0 )
		{
			powerInput = p;
		}
		else
		{
			powerInput = 0;
		}
	}
	
	public void setBrake( double b )
	{
		bBraking = true;
		bEBraking = false;
		powerInput = 0;
		
		if( bBrakeFail )
		{
			brakeInput = 0;
		}
		else
		{
			brakeInput = b;
		}
	}
	
	public void setEBrake()
	{
		bBraking = false;
		bEBraking = true;
	}
	
	public double run(double tickLen, double mass, double grade )
	{
		// Convert mass in lbs to kg
		double dMassInkg = mass*(0.453592);
		double dVelocityToReturn = 0; // in mph
		
		if(bBraking)
		{
			if(bBrakeFail)
			{
				dVelocityToReturn = runPower(0, tickLen, dMassInkg, grade);
			}
			else
			{
				dVelocityToReturn = runBrake(brakeInput, tickLen, dMassInkg); // TODO: factor in grade
			}
		}
		else if(bEBraking)
		{
			dVelocityToReturn = runEBrake(tickLen, dMassInkg); // TODO: factor in grade
		}
		else
		{
			dVelocityToReturn = runPower(powerInput, tickLen, dMassInkg, grade);
		}
		
		bBraking = false;
		bEBraking = false;
		brakeInput = 0;
		powerInput = 0;
		
		logFile.write("Train " + iTrainID + ": PhysEng: Updated the velocity to " + dVelocityToReturn);
		
		return dVelocityToReturn;
	}
	
	// power gets input, returns velocity
	private double runPower( double power, double tickLen, double mass, double grade )
	{
		double dLocalVelocity = currVelocity;
		//double dOldVelocity = currVelocity;
		
		logFile.write("PhysEng: Inside runPower()");
		if( power < 0 )
		{
			return dLocalVelocity;
		}
		
		if( power > TrainConstants.dMaxPower )
		{
			power= TrainConstants.dMaxPower;
		}
		
		double actualT = tickLen/tickDivideBy;
		
		// Do grade calculations
		double dAngle = Math.atan(grade);
		double dForceFromGrade = (mass)*(TrainConstants.dGravityAccel)*Math.sin(dAngle);
		
		// Do friction calculations
		double dNormalForce = (mass)*(TrainConstants.dGravityAccel)*Math.cos(dAngle);
		double dForceOfFriction = (TrainConstants.dNaturalEngineFrictionCoeff)*(dNormalForce);
		
		// Do wheel slippage calculations
		double dWheelToTrackForceOfFriction = 0;
		if(bWheelSlip)
		{
			dWheelToTrackForceOfFriction = (dFrictionCoeffKinetic) * (dNormalForce);
		}
		else
		{
			dWheelToTrackForceOfFriction = (dFrictionCoeffStatic) * (dNormalForce);
		}
		
		for( int i = 0; i < tickDivideBy; i++ )
		{
			// Step 1: Divide by the current Velocity
			double dForce;
			if( dLocalVelocity < .1 )
			{
				dForce = power/.1;
			}
			else
			{
				dForce = power/dLocalVelocity;
			}
			
			// Subtract out natural friction and gravity due to grade
			dForce -= dForceFromGrade;
			dForce -= dForceOfFriction;
			
			// Take care of wheel slip
			if( dWheelToTrackForceOfFriction < dForce )
			{
				bWheelSlip = true;
				dForce = (-1) * (dForceOfFriction + dForceFromGrade);
			}
			else
			{
				bWheelSlip = false;
			}
			
			// Get this instantaneous acceleration from the force
			double instAccel = dForce / mass;
			
			// Step 4: Use this acceleration for the next tick length to get velocity
			double dChangeInVelocity = (instAccel)*(actualT);
			dLocalVelocity = dLocalVelocity + dChangeInVelocity;
		}
		
		if( dLocalVelocity > TrainConstants.dMaxSpeed)
		{
			// Convert currVelocity from m/s to mph
			dCurrentAccel = ((TrainConstants.dMaxSpeed - this.currVelocity)/tickLen) * (3.28084);
			this.currVelocity = TrainConstants.dMaxSpeed;
			double dVelocityInImperial = TrainConstants.dMaxSpeed*2.23694;
			return dVelocityInImperial;
		}
		else if( dLocalVelocity < 0 )
		{
			dCurrentAccel = ((0 - this.currVelocity)/tickLen) * (3.28084);
			this.currVelocity = 0;
			return 0;
		}
		else
		{
			// Convert currVelocity from m/s to mph
			dCurrentAccel = ((dLocalVelocity - this.currVelocity)/tickLen) * (3.28084);
			this.currVelocity = dLocalVelocity;
			double dVelocityInImperial = dLocalVelocity*2.23694;
			return dVelocityInImperial;
		}
	}
	
	private double runBrake( double percentOn, double tickLen, double mass)
	{
		logFile.write("Train " + iTrainID + ": PhysEng: Inside runBrake()");
		double dLocalVelocity = this.currVelocity;
		
		if( (percentOn < 0) || (percentOn > 100) )
		{
			logFile.write("PhysEng: Invalid brake input. Returning...");
			return dLocalVelocity;
		}
		
		//double dOldVelocity = dLocalVelocity;
		double actualT = tickLen;
		double actualDecel = (percentOn / 100) * TrainConstants.dMaxServiceBreakDecel;
		dCurrentAccel = (-1) * actualDecel;
		
		dLocalVelocity = dLocalVelocity - (actualDecel)*(actualT);
			
		if( dLocalVelocity <= 0)
		{
			dLocalVelocity = 0;
		}
		
		this.currVelocity = dLocalVelocity;

		// Convert currVelocity from m/s to mph
		double dVelocityInImperial = dLocalVelocity*2.23694;
		return dVelocityInImperial;
	}
	
	private double runEBrake( double tickLen, double mass)
	{
		logFile.write("Train " + iTrainID + ": PhysEng: Inside runEBrake()");
		
		double dLocalVelocity = this.currVelocity;
		//double dOldVelocity = dLocalVelocity;
		double actualT = tickLen;
		double actualDecel = TrainConstants.dMaxEBreakDecel;
		dCurrentAccel = (-1) * actualDecel;
		
		dLocalVelocity = dLocalVelocity - (actualDecel)*(actualT);
			
		if( dLocalVelocity <= 0)
		{
			dLocalVelocity = 0;
		}

		this.currVelocity = dLocalVelocity;
		
		// Convert currVelocity from m/s to mph
		double dVelocityInImperial = dLocalVelocity*2.23694;
		return dVelocityInImperial;
	}
}
